#include "math.h"

#include <QtMath>

Math::Math()
{
}

std::vector<double> Math::robotToWorld(const std::vector<double> &src)
{
    // 无效数据源
    if (src.size() < 3) {
        return std::vector(6, 0.);
    }

    // 机械臂坐标系偏移
    static const QMatrix4x4 robot_matrix_ = { 0, 1, 0, 0, //
                                              0, 0, 1, 0, //
                                              1, 0, 0, 0, //
                                              0, 0, 0, 1 };
    QMatrix4x4 matrix = robot_matrix_;

    // 平移
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            matrix(i, 3) += robot_matrix_(i, j) * src[j] * 1000.;
        }
    }

    // 只有 x, y, z
    if (src.size() != 6) {
        auto center = matrix.column(3).toVector3D();
        return { center.x(), center.y(), center.z() };
    }

    // 旋转
    matrix.rotate(qRadiansToDegrees(src[5]), 0.0, 0.0, 1.0);
    matrix.rotate(qRadiansToDegrees(src[4]), 0.0, 1.0, 0.0);
    matrix.rotate(qRadiansToDegrees(src[3]), 1.0, 0.0, 0.0);

    // 完整的 x, y, z, rx, ry, rz
    return matrixToPose(matrix);
}

std::vector<double> Math::matrixToPose(const QMatrix4x4 &matrix)
{
    float rx, ry, rz;

    double eps = 1e-16;
    // old ZYX order (as per Paul book)
    if (fabs(matrix(0, 0)) < eps && fabs(matrix(1, 0)) < eps) {
        // singularity
        rx = atan2(-matrix(1, 2), matrix(1, 1));
        ry = atan2(-matrix(2, 0), matrix(0, 0));
        rz = 0.0;
    } else {
        rx = atan2(matrix(2, 1), matrix(2, 2));
        ry = atan2(-matrix(2, 0), sqrt(matrix(0, 0) * matrix(0, 0) +
                                       matrix(1, 0) * matrix(1, 0)));
        rz = atan2(matrix(1, 0), matrix(0, 0));
    }

    return std::vector<double>{ matrix(0, 3), matrix(1, 3), matrix(2, 3),
                                rx,           ry,           rz };
}
